//
// Created by whitby on 8/4/22.
//

#ifndef DENSESURFELMAPPING_FUSION_FUNCTIONS_H
#define DENSESURFELMAPPING_FUSION_FUNCTIONS_H

#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>

#include "elements.h"

const int ITERATION_NUM = 3;
const int THREAD_NUM = 10;
const int SP_SIZE = 8;
const float MAX_ANGLE_COS = 0.1;
// for drive
const float HUBER_RANGE = 0.4;
const float BASELINE = 0.5;
const float DISPARITY_ERROR = 4.0;
const float MIN_TOLERATE_DIFF = 0.1;

class FusionFunctions {
private:
    float fx, fy, cx, cy;
    int image_width, image_height;
    int sp_width, sp_height;    // what's this?
    float fuse_far, fuse_near;

    cv::Mat image;
    cv::Mat depth;

    std::vector<float> space_map;
    std::vector<float> norm_map;
    std::vector<Superpixel_seed> superpixel_seeds;
    std::vector<int> superpixel_index;

    std::vector<std::shared_ptr<SurfelElement>> *local_surfels_ptr;
    std::vector<std::shared_ptr<SurfelElement>> *new_surfels_ptr;

    // get the super pixels
    void generate_super_pixels();

    void back_project(
            const float &u, const float &v, const float &depth, float &x, float &y, float &z) const;

    bool calculate_cost(
            float &no_depth_cost, float &depth_cost,
            const float &pixel_intensity, const float &pixel_inverse_depth,
            const int &x, const int &y,
            const int &sp_x, const int &sp_y);

    void update_pixels_kernel(int thread_i, int thread_num);

    void update_pixels();

    void update_seeds_kernel(
            int thread_i, int thread_num);

    void update_seeds();

    void initialize_seeds_kernel(
            int thread_i, int thread_num);

    static void get_huber_norm(
            float &nx, float &ny, float &nz, float &nb,
            std::vector<float> &points);

    void initialize_seeds();

    void calculate_spaces_kernel(int thread_i, int thread_num);

    void calculate_sp_depth_norms_kernel(int thread_i, int thread_num);

    void calculate_pixels_norms_kernel(int thread_i, int thread_num);

    void calculate_norms();

    // for fusion
    void fuse_surfels_kernel(
            int thread_i, int thread_num,
            int reference_frame_index,
            Eigen::Matrix4f pose,
            Eigen::Matrix4f inv_pose);

    void initialize_surfels(
            int reference_frame_index,
            Eigen::Matrix4f pose);

    void project(float &x, float &y, float &z, float &u, float &v) const;

    static float get_weight(float &depth);

public:
    void initialize(
            int _width, int _height,
            float _fx, float _fy, float _cx, float _cy,
            float _fuse_far, float _fuse_near);

    void fuse_initialize_map(
            int reference_frame_index,
            cv::Mat &input_image,
            cv::Mat &input_depth,
            Eigen::Matrix4f &pose,
            std::vector<std::shared_ptr<SurfelElement>> &local_surfels,
            std::vector<std::shared_ptr<SurfelElement>> &new_surfels);
};

#endif //DENSESURFELMAPPING_FUSION_FUNCTIONS_H
